#include <Arduino.h>

// 修正引脚定义
#define L_EN_PIN_L    32  // 左侧使能
#define R_EN_PIN_L    33  // 左侧使能
#define L_PWM_PIN_L   25  // 左侧正转PWM
#define R_PWM_PIN_L   26  // 左侧反转PWM
#define L_EN_PIN_R    5   // 右侧使能
#define R_EN_PIN_R    17  // 右侧使能
#define L_PWM_PIN_R   16  // 右侧正转PWM
#define R_PWM_PIN_R   4   // 右侧反转PWM
#define POTENTIOMETER_PIN 34  // 电位器模拟输入
#define DIR_ANALOG_PIN 2    // 方向摇杆模拟输入
#define DIR_POS_PIN   27     // 正向数字开关
#define DIR_NEG_PIN   14     // 反向数字开关

// 使用四个独立PWM通道
#define PWM_CH_L_L 0  // 左电机正转
#define PWM_CH_R_L 1  // 左电机反转
#define PWM_CH_L_R 2  // 右电机正转
#define PWM_CH_R_R 3  // 右电机反转

const int freq = 25000;    // 25kHz PWM频率
const int resolution = 8;  // 8位分辨率
int speed = 0;
int direction_val = 1850;  // 摇杆中值

void setup() {
  Serial.begin(115200);
  
  // 初始化使能引脚
  pinMode(L_EN_PIN_L, OUTPUT);
  pinMode(R_EN_PIN_L, OUTPUT);
  pinMode(L_EN_PIN_R, OUTPUT);
  pinMode(R_EN_PIN_R, OUTPUT);
  digitalWrite(L_EN_PIN_L, HIGH);
  digitalWrite(R_EN_PIN_L, HIGH);
  digitalWrite(L_EN_PIN_R, HIGH);
  digitalWrite(R_EN_PIN_R, HIGH);

  // 方向开关引脚（启用内部上拉电阻）
  pinMode(DIR_POS_PIN, INPUT_PULLUP);
  pinMode(DIR_NEG_PIN, INPUT_PULLUP);

  // 配置四个独立PWM通道
  ledcSetup(PWM_CH_L_L, freq, resolution);
  ledcSetup(PWM_CH_R_L, freq, resolution);
  ledcSetup(PWM_CH_L_R, freq, resolution);
  ledcSetup(PWM_CH_R_R, freq, resolution);
  
  // 正确连接PWM引脚
  ledcAttachPin(L_PWM_PIN_L, PWM_CH_L_L);  // 左正
  ledcAttachPin(R_PWM_PIN_L, PWM_CH_R_L);  // 左反
  ledcAttachPin(L_PWM_PIN_R, PWM_CH_L_R);  // 右正
  ledcAttachPin(R_PWM_PIN_R, PWM_CH_R_R);  // 右反
}

// 改进的电机控制函数
void setMotor(int speed_val, int dir_pos, int dir_neg, int steer) {
  // 限制速度范围
  speed_val = constrain(speed_val, 0, 255);
  
  // 初始化电机速度
  int left_speed = speed_val;
  int right_speed = speed_val;
  
  // 计算转向因子 (0-1)
  float steer_factor = 0;
  if (steer > 1900) {        // 左转
    steer_factor = (steer - 1900) / 2195;  
    left_speed *= (1.0 - steer_factor);
  } 
  else if (steer < 1700) {   // 右转
    steer_factor = (1750 - steer) / 1800.0;
    right_speed *= (1.0 - steer_factor);
  }

  // 应用方向控制
  if (dir_pos == LOW && dir_neg == HIGH) {  // 正转
    // 左电机正转
    ledcWrite(PWM_CH_L_L, left_speed);
    ledcWrite(PWM_CH_R_L, 0);
    // 右电机正转
    ledcWrite(PWM_CH_L_R, right_speed);
    ledcWrite(PWM_CH_R_R, 0);
  } 
  else if (dir_pos == HIGH && dir_neg == LOW) {  // 反转
    // 左电机反转
    ledcWrite(PWM_CH_L_L, 0);
    ledcWrite(PWM_CH_R_L, left_speed);
    // 右电机反转
    ledcWrite(PWM_CH_L_R, 0);
    ledcWrite(PWM_CH_R_R, right_speed);
  } 
  else {  // 停止
    ledcWrite(PWM_CH_L_L, 0);
    ledcWrite(PWM_CH_R_L, 0);
    ledcWrite(PWM_CH_L_R, 0);
    ledcWrite(PWM_CH_R_R, 0);
  }
}

void loop() {
  // 读取传感器值
  direction_val = analogRead(DIR_ANALOG_PIN);
  int pot_value = analogRead(POTENTIOMETER_PIN);
  speed = map(pot_value, 0, 4095, 0, 255);
  
  int dir_pos = digitalRead(DIR_POS_PIN);
  int dir_neg = digitalRead(DIR_NEG_PIN);

  // 调试输出
  Serial.print("Pot:");
  Serial.print(pot_value);
  Serial.print(" Spd:");
  Serial.print(speed);
  Serial.print(" Dir:");
  Serial.print(direction_val);
  Serial.print(" POS:");
  Serial.print(dir_pos);
  Serial.print(" NEG:");
  Serial.println(dir_neg);

  // 更新电机控制
  setMotor(speed, dir_pos, dir_neg, direction_val);
  delay(50);
}